#include <Arduino.h>
#define BLINKER_WIFI

#include <Blinker.h>
#include "ESP32_CAM_SERVER.h"

char auth[] = "e139f5a0d871";
char ssid[] = "Sweet_meng";
char pswd[] = "aita0116.";

int freq = 50;         // 频率(20ms周期)
int servo_channel = 8; // 通道(高速通道（0 ~ 7）由80MHz时钟驱动，低速通道（8 ~ 15）由 1MHz 时钟驱动。)
int motorA1_channel = 9;
int motorA2_channel = 10;
int motorB1_channel = 11;
int motorB2_channel = 12;
int flashled_channel = 13;
int resolution_bit = 8; // 分辨率

int flashled_pin = 4; //闪光灯
int servo_pin = 2;    //舵机控制管脚
int motorA1_pin = 14; //电机控制管脚A1
int motorA2_pin = 15; //电机控制管教A2
int motorB1_pin = 13; //电机控制管脚B1
int motorB2_pin = 12; //电机控制管教B2

int angle, motor_pwm, flashled_pwm, x, y;

BlinkerSlider Slider1("hk1"); //舵机角度
BlinkerSlider Slider2("hk2"); //电机PWM步进
BlinkerSlider Slider3("hk3"); //闪光灯亮度

BlinkerJoystick JOY1("JOYKey"); //遥杆

bool setup_camera = false;

//void heartbeat()
//{
//    Slider1.print(BRIGHTNESS);
//    Slider2.print(UPDATES_PER_SECOND);
//    RGBWS2812.print(colorR, colorG, colorB, BRIGHTNESS);
//}
void dataRead(const String &data)
{
    BLINKER_LOG("Blinker readString: ", data);

    Blinker.vibrate();

    uint32_t BlinkerTime = millis();

    Blinker.print("millis", BlinkerTime);

    Slider1.color("#FFFFFF");
    Slider1.print(random(1, 100));
}

void slider1_callback(int32_t value)
{
    angle = value;
    BLINKER_LOG("get 舵机角度 value: ", value);
}
void slider2_callback(int32_t value)
{
    motor_pwm = value;
    BLINKER_LOG("get motor_pwm value: ", value);
}
void slider3_callback(int32_t value)
{
    flashled_pwm = value;
    BLINKER_LOG("get flashled_pwm value: ", value);
}
void joystick1_callback(uint8_t xAxis, uint8_t yAxis)
{
    BLINKER_LOG("Joystick1 X axis: ", xAxis);
    BLINKER_LOG("Joystick1 Y axis: ", yAxis);
    x = xAxis;
    y = yAxis;
}

int calculatePWM(int degree) //舵机角度
{
    //20ms周期，高电平0.5-2.5ms，对应0-180度角度
    const float deadZone = 6.4; //对应0.5ms（0.5ms/(20ms/256）)
    const float max = 32;       //对应2.5ms
    if (degree < 0)
        degree = 0;
    if (degree > 180)
        degree = 180;
    return (int)(((max - deadZone) / 180) * degree + deadZone);
}

void servo_360() //360度舵机控制
{
    if (y < 128)
    {
        ledcWrite(servo_channel, calculatePWM(88)); // 输出PWM
    }
    else if (y == 128)
    {
        ledcWrite(servo_channel, calculatePWM(92)); // 输出PWM
    }
    else if (y > 128)
    {
        ledcWrite(servo_channel, calculatePWM(96)); // 输出PWM
    }
}

void xunhuan()
{
    ledcWrite(servo_channel, calculatePWM(angle)); // 输出PWM
    ledcWrite(flashled_channel, flashled_pwm);     // 输出PWM
    if ((y <= 37.5) && (x >= 37.5) && (x <= 217.5))//前进
    {
        ledcWrite(motorA1_channel, motor_pwm);
        ledcWrite(motorA2_channel, 0);
        ledcWrite(motorB1_channel, motor_pwm);
        ledcWrite(motorB2_channel, 0);
    }
    if ((x <= 37.5) && (y >= 37.5) && (y <= 217.5))//左转
    {
        ledcWrite(motorA1_channel, 0);
        ledcWrite(motorA2_channel, motor_pwm);
        ledcWrite(motorB1_channel, motor_pwm);
        ledcWrite(motorB2_channel, 0);
    }
    if ((x >= 217.5) && (y >= 37.5) && (y <= 217.5))//右转
    {
        ledcWrite(motorA1_channel, motor_pwm);
        ledcWrite(motorA2_channel, 0);
        ledcWrite(motorB1_channel, 0);
        ledcWrite(motorB2_channel, motor_pwm);
    }
    if ((y >= 217.5) && (x >= 37.5) && (x <= 217.5))//后退
    {
        ledcWrite(motorA1_channel, 0);
        ledcWrite(motorA2_channel, motor_pwm);
        ledcWrite(motorB1_channel, 0);
        ledcWrite(motorB2_channel, motor_pwm);
    }
     if ((x > 37.5) && (x < 217.5) && (y > 37.5) && (y < 217.5))//后退
    {
        ledcWrite(motorA1_channel, 0);
        ledcWrite(motorA2_channel, 0);
        ledcWrite(motorB1_channel, 0);
        ledcWrite(motorB2_channel, 0);
    }
}

void setup()
{
    Serial.begin(115200);
    BLINKER_DEBUG.stream(Serial);

    Blinker.begin(auth, ssid, pswd);
    Blinker.attachData(dataRead);

    Slider1.attach(slider1_callback);
    Slider2.attach(slider2_callback);
    Slider3.attach(slider3_callback);
    JOY1.attach(joystick1_callback);
    
    

    ledcSetup(servo_channel, freq, resolution_bit);    // 设置通道
    ledcSetup(motorA1_channel, freq, resolution_bit);  // 设置通道
    ledcSetup(motorA2_channel, freq, resolution_bit);  // 设置通道
    ledcSetup(motorB1_channel, freq, resolution_bit);  // 设置通道
    ledcSetup(motorB2_channel, freq, resolution_bit);  // 设置通道
    ledcSetup(flashled_channel, 400, resolution_bit); // 设置通道

    ledcAttachPin(servo_pin, servo_channel);      // 将通道与对应的引脚连接
    ledcAttachPin(motorA1_pin, motorA1_channel);  // 将通道与对应的引脚连接
    ledcAttachPin(motorA2_pin, motorA2_channel);  // 将通道与对应的引脚连接
    ledcAttachPin(motorB1_pin, motorB1_channel);  // 将通道与对应的引脚连接
    ledcAttachPin(motorB2_pin, motorB2_channel);  // 将通道与对应的引脚连接
    ledcAttachPin(flashled_pin, flashled_channel); // 将通道与对应的引脚连接

    ledcWrite(servo_channel, calculatePWM(90));
    ledcWrite(motorA1_channel, 0);
    ledcWrite(motorA2_channel, 0);
    ledcWrite(motorB1_channel, 0);
    ledcWrite(motorB2_channel, 0);
    ledcWrite(flashled_channel, 0);
}

void loop()
{
    Blinker.run();
    
    if (Blinker.connected() && !setup_camera)
    {
        setupCamera();
        setup_camera = true;
        Blinker.printObject("video", "{\"str\":\"mjpg\",\"url\":\"http://" + WiFi.localIP().toString() + "\"}");
    }
xunhuan();
}
